#!/usr/bin/env python3
# -*- coding: utf-8 -*-
import rospy
from std_msgs.msg import Int32  # 使用标准消息类型 Int32
import serial
import os

def callback(data):
    # 使用 sudo 权限设置串口权限
    os.system('echo yy330 | sudo -S chmod a+rw /dev/ttyTHS1')

    try:
        # 配置串口
        ser = serial.Serial('/dev/ttyTHS1', 9600, timeout=1)

        # 检查 data 是否在合法范围内
        if 0 <= data.data <= 9999:  # data.data 是 Int32 消息的整数值
            # 提取舵机编号和角度
            servo_num = data.data // 1000      # 获取舵机编号（1, 2, 或 3）
            angle = data.data % 1000          # 获取角度（0-999）

            # 校验舵机编号
            if servo_num < 1 or servo_num > 3:
                rospy.logwarn("Invalid servo number: {}".format(servo_num))
                ser.close()
                return

            # 组装成四位命令，例如 1030 表示舵机1转30度
            command = "{:1d}{:03d}".format(servo_num, angle)  # 确保命令为四位数字

            # 发送四个字节（每个字节对应一个字符）
            ser.write(command.encode('utf-8'))
            rospy.loginfo("Sent: {}".format(command))

            # 等待回复
            response = ser.read(1)  # 读取一个字节的反馈

            # 检查响应是否为空
            if response:
                # 将接收到的字节转换为整数
                received_int = response[0]
                if received_int == 1:
                    rospy.loginfo("Servo command executed successfully.")
                else:
                    rospy.logwarn("Servo command failed.")
            else:
                rospy.loginfo("No response received.")
        else:
            rospy.logwarn("Invalid value received, ignoring...")

        # 关闭串口
        ser.close()

    except serial.SerialException as e:
        rospy.logerr("Error occurred: %s", e)

def listener():
    rospy.init_node('duoji_subscriber', anonymous=True)

    # 订阅 duoji_topic 主题，消息类型为 Int32
    rospy.Subscriber("duoji_topic", Int32, callback, queue_size=1)

    # 循环等待回调函数
    rospy.spin()

if __name__ == '__main__':
    listener()

